// standard libs
//ff_utils.c
#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>


#include "ff_utils.h"
#include "main.h"
#include "../utils/util.h"
#include "../utils/button.h"
#include "../utils/LCD_driver.h"
#include "../utils/LCD_functions.h"


unsigned char off = 0x00; // this is used to correct the compass if there is metal in the floor

unsigned char GotFlame=0;

unsigned char TurnToHeadingSpeed = 4;

/**********************************************************************************************************
###########################################################################################################
###########################################################################################################
###########################################################################################################
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 	

Goes to and Home from each of the rooms

^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
###########################################################################################################
###########################################################################################################
###########################################################################################################
***********************************************************************************************************/

/**********************************************************************************************************
*
* 	Goes To Room 1 Trinity
*
***********************************************************************************************************/

//#########################################################################################################
void ToRoom1(void)
{

	Forward(9,UNTIL_TIME,TimerMsCur() + 400);	
	
	//sendString("Following Right Wall");
	//sendString("\r\n");	
	
	LWallFollow(0x40,UNTIL_LINESENSOR_LOWER,0x12);
	
	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
 	TimerWait(300);
	
	//sendString("Sees Line");
	
	GoToHeading((RoomOneEnterHeading + off), TurnToHeadingSpeed); 
	GoToHeading((RoomOneEnterHeading + off), TurnToHeadingSpeed); 
	GoToHeading((RoomOneEnterHeading + off), TurnToHeadingSpeed); 

	//printLCD("entered");	
	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
 	TimerWait(300);	

	Forward(8,UNTIL_TIME,TimerMsCur() + 400);
	
	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
	
	sendString("Entered Room One\r\r"); 

 	TimerWait(200);
	
	GoToHeading((RoomOneCornerHeading + off), TurnToHeadingSpeed); 
	GoToHeading((RoomOneCornerHeading + off), TurnToHeadingSpeed); 
	GoToHeading((RoomOneCornerHeading + off), TurnToHeadingSpeed); 
	
	//printLCD("entered");	
	
	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
		
	sendString("scanning For Flame\r\r"); 

 	TimerWait(200);
	
	if(ScanRoomForFlame())
	{	
		sendString("Found Flame\r\r"); 
		
		TimerWait(50);
		
		GoToFlame();
		SetLeftMotorPWM(0);
		SetRightMotorPWM(0);
		TimerWait(300);
		HomeFrom1();
	}
	else
	{
		sendString("No Flame\r\r"); 
		//TimerWait(50);
		//sendString("No Flame");
		//sendString("\r\n");	

		//printLCD("Back from Check for flame");	
		//TimerWait(5000);	
		//printLCD("Turn to leave");	
		
		GoToHeading((RoomOneExitHeading + off), TurnToHeadingSpeed); 
		GoToHeading((RoomOneExitHeading + off), TurnToHeadingSpeed); 
		GoToHeading((RoomOneExitHeading + off), TurnToHeadingSpeed); 
		TimerWait(300);
		
		Forward(6,UNTIL_LINESENSOR_LOWER,0x12);

		//sendString("Done");
		//sendString("\r\n");	

		//printLCD("Done");	
		
		SetLeftMotorPWM(0);
		SetRightMotorPWM(0);
		TimerWait(300);
	
		// goes To Room 2 from room 1 door white line
		
		ToRoom2();
	}
	printLCD("Exit");
}
//#########################################################################################################





/**********************************************************************************************************
*
* 	Goes To Room 2 Trinity
*
***********************************************************************************************************/

//#########################################################################################################
void ToRoom2(void)//go to room 2 from room 1
{
	GoToHeading(((RoomOneExitHeading-4) + off), TurnToHeadingSpeed); // turns a little more because the motors drift to the right
	GoToHeading(((RoomOneExitHeading-4) + off), TurnToHeadingSpeed); 
	GoToHeading(((RoomOneExitHeading-4) + off), TurnToHeadingSpeed); 
	
	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
	TimerWait(300);
	
	Forward(8,UNTIL_TIME,TimerMsCur() + 300);
	Forward(8, UNTIL_MIDDLESENSOR_LOWER,SeeWallFront);
	
	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);

	sendString("Left Room One\r\r"); 

 	TimerWait(200);
	
	GoToHeading((RoomTwoExitHeading + off), TurnToHeadingSpeed); 
	GoToHeading((RoomTwoExitHeading + off), TurnToHeadingSpeed); 
	GoToHeading((RoomTwoExitHeading + off), TurnToHeadingSpeed);
	
	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
 	TimerWait(300);
	
	LWallFollow(0x3A,UNTIL_LINESENSOR_LOWER,0x12);
	
	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
 	TimerWait(300);		
	
	GoToHeading((RoomTwoEnterHeading + off), TurnToHeadingSpeed); 
	GoToHeading((RoomTwoEnterHeading + off), TurnToHeadingSpeed); 
	GoToHeading((RoomTwoEnterHeading + off), TurnToHeadingSpeed);
 
	//printLCD("entered");	
	
	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
 	TimerWait(300);	
	
	Forward(8,UNTIL_TIME,TimerMsCur() + 400);	

	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
 
 	sendString("Entered Room Two\r\r"); 
	
 	TimerWait(200);
	
	GoToHeading((RoomTwoCornerHeading + off), TurnToHeadingSpeed); 
	GoToHeading((RoomTwoCornerHeading + off), TurnToHeadingSpeed); 
	GoToHeading((RoomTwoCornerHeading + off), TurnToHeadingSpeed); 
	
	//printLCD("entered");	
	
	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
	
	sendString("Scanning For Flame\r\r"); 

 	TimerWait(200);

	if(ScanRoomForFlame())
	{	
		sendString("Found Flame\r\r"); 

		GoToFlame();
		SetLeftMotorPWM(0);
		SetRightMotorPWM(0);
		TimerWait(300);
		HomeFrom2();
	}
	else
	{
		//sendString("No Flame");
		//sendString("\r\n");	

		//printLCD("Back from Check for flame");	
		//TimerWait(5000);	
		//printLCD("Turn to leave");	
		GoToHeading((RoomTwoExitHeading + off), TurnToHeadingSpeed); 
		GoToHeading((RoomTwoExitHeading + off), TurnToHeadingSpeed); 
		GoToHeading((RoomTwoExitHeading + off), TurnToHeadingSpeed); 

		SetLeftMotorPWM(0);
		SetRightMotorPWM(0);
		
		sendString("No Flame\r\r"); 

		TimerWait(200);

		Forward(6,UNTIL_LINESENSOR_LOWER,0x12);
		//sendString("Done");
		//sendString("\r\n");
	
		SetLeftMotorPWM(0);
		SetRightMotorPWM(0);
		TimerWait(300);

		printLCD("Done");	
//		TimerWait(500);
		// goes To Room 2 from room 1 door white line
		ToRoom3();
	}	
	
	printLCD("Exit");
}		

//#########################################################################################################

/**********************************************************************************************************
*
* 	Goes To Room 3 Trinity
*
***********************************************************************************************************/

//#########################################################################################################
void ToRoom3(void)//go to room 3 from room 2
{
	Forward(8,UNTIL_TIME,TimerMsCur() + 300);
	
	Forward(8, UNTIL_MIDDLESENSOR_LOWER,SeeWallFront);
	
	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
	
 	sendString("Left Room Two\r\r"); 

 	TimerWait(200);
	
	GoToHeading((RoomOneExitHeading + off), TurnToHeadingSpeed); 
	GoToHeading((RoomOneExitHeading + off), TurnToHeadingSpeed); 
	GoToHeading((RoomOneExitHeading + off), TurnToHeadingSpeed); 
	
	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
	TimerWait(300);
	
	//GoToHeading((RoomThreeEnterHeading), 8);
	// *** change to lower angle to center in doorway
	//TimerWait(1000);
	RWallFollow(0x3A,UNTIL_LINESENSOR_LOWER,0x12);
	
 	sendString("Entered Room Three\r\r"); 

 	TimerWait(200);	
	
	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
 	TimerWait(300);
	
	GoToHeading((RoomThreeEnterHeading + off), TurnToHeadingSpeed); 
	GoToHeading((RoomThreeEnterHeading + off), TurnToHeadingSpeed); 
	GoToHeading((RoomThreeEnterHeading + off), TurnToHeadingSpeed); 
	
	//printLCD("entered");	
	
	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
 	TimerWait(300);	
	
	Forward(8,UNTIL_TIME,TimerMsCur() + 400);	
	
	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
 	TimerWait(300);
	
	GoToHeading((RoomThreeCornerHeading + off), TurnToHeadingSpeed); 
	GoToHeading((RoomThreeCornerHeading + off), TurnToHeadingSpeed); 
	GoToHeading((RoomThreeCornerHeading + off), TurnToHeadingSpeed); 
	
	//printLCD("Turned To Corner");
	
	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
 	TimerWait(200);

	sendString("Scanning For Flame\r\r"); 


	if(ScanRoomForFlame())
	{	
		sendString("Found Flame\r\r"); 

		GoToFlame();
		SetLeftMotorPWM(0);
		SetRightMotorPWM(0);
		TimerWait(300);
		HomeFrom3();
	}
	else
	{
	//	sendString("No Flame");
	//	sendString("\r\n");	

		//printLCD("Back from Check for flame");	
		//TimerWait(5000);	
	//	printLCD("Turn to leave");	
	
		GoToHeading((RoomThreeExitHeading + off), TurnToHeadingSpeed); 
		GoToHeading((RoomThreeExitHeading + off), TurnToHeadingSpeed); 
		GoToHeading((RoomThreeExitHeading + off), TurnToHeadingSpeed); 
		
		SetLeftMotorPWM(0);
		SetRightMotorPWM(0);
		TimerWait(300);
		
		Forward(6,UNTIL_LINESENSOR_LOWER,0x12);
	
		SetLeftMotorPWM(0);
		SetRightMotorPWM(0);
		sendString("No Flame\r\r"); 

		TimerWait(200);

		GoToHeading((RoomThreeExitHeading + off), TurnToHeadingSpeed); 
		GoToHeading((RoomThreeExitHeading + off), TurnToHeadingSpeed); 
		GoToHeading((RoomThreeExitHeading + off), TurnToHeadingSpeed);
		
	//	sendString("Done");
	//	sendString("\r\n");
	
		SetLeftMotorPWM(0);
		SetRightMotorPWM(0);
		TimerWait(300);

	//	printLCD("Done");	
	//	TimerWait(300);
		// goes To Room 2 from room 1 door white line
		ToRoom4();
	}

	printLCD("Exit");

}
//#########################################################################################################



/**********************************************************************************************************
*
* 	Goes To Room 4 Trinity
*
***********************************************************************************************************/

//#########################################################################################################
void ToRoom4(void)//go to room 4 from room 3
{
	Forward(8,UNTIL_TIME,TimerMsCur() + 200);
	
	Forward(8, UNTIL_MIDDLESENSOR_LOWER,SeeWallFront);
	
	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
 	sendString("Left Room Three\r\r"); 
	
 	TimerWait(200);
	
	GoToHeading((0x7D + off), TurnToHeadingSpeed); 
	GoToHeading((0x7D + off), TurnToHeadingSpeed); 
	GoToHeading((0x7D + off), TurnToHeadingSpeed); 

	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
	TimerWait(300);
	
	Forward(8,UNTIL_TIME,TimerMsCur() + 500);

	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
	TimerWait(300);

	LWallFollow(0x3A,UNTIL_MIDDLESENSOR_LOWER,SeeWallFront);
	
	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
 	TimerWait(300);

	GoToHeading((RoomFourEnterHeading + off), TurnToHeadingSpeed); 
	GoToHeading((RoomFourEnterHeading + off), TurnToHeadingSpeed); 
	GoToHeading((RoomFourEnterHeading + off), TurnToHeadingSpeed); 
	
	//printLCD("entered");
	
	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
 	TimerWait(300);	
	
	Forward(6,UNTIL_LINESENSOR_LOWER,0x12);
	
	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
 	sendString("Entered Room Four\r\r"); 

 	TimerWait(200);

	GoToHeading((RoomFourEnterHeading + off), TurnToHeadingSpeed); 
	GoToHeading((RoomFourEnterHeading + off), TurnToHeadingSpeed); 
	GoToHeading((RoomFourEnterHeading + off), TurnToHeadingSpeed); 
		
	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
 	TimerWait(300);	
		
	Forward(8,UNTIL_TIME,TimerMsCur() + 400);	
	
	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
	TimerWait(300);
	
	GoToHeading((RoomFourCornerHeading + off), TurnToHeadingSpeed); 
	GoToHeading((RoomFourCornerHeading + off), TurnToHeadingSpeed); 
	GoToHeading((RoomFourCornerHeading + off), TurnToHeadingSpeed);
 
	//printLCD("entered");	

	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);

	sendString("Scanning For Flame\r\r"); 

 	TimerWait(200);

	if(ScanRoomForFlame())
	{	
		sendString("Found Flame\r\r"); 
		GotFlame = 1;
		GoToFlame();
		SetLeftMotorPWM(0);
		SetRightMotorPWM(0);
		TimerWait(300);
		HomeFrom4();
	}
	else
	{
	//	sendString("No Flame");
	//	sendString("\r\n");	

		//printLCD("Back from Check for flame");	
		//TimerWait(5000);	
	//	printLCD("Turn to leave");	
	
		GoToHeading((RoomFourExitHeading + off), TurnToHeadingSpeed); 
		GoToHeading((RoomFourExitHeading + off), TurnToHeadingSpeed); 
		GoToHeading((RoomFourExitHeading + off), TurnToHeadingSpeed); 
		
		TimerWait(300);
		Forward(6,UNTIL_LINESENSOR_LOWER,0x12);
	//	sendString("Done");
	//	sendString("\r\n");	

	//	printLCD("Done");	
		
		SetLeftMotorPWM(0);
		SetRightMotorPWM(0);
		sendString("No Flame Error Missed Flame\r\r"); 

		TimerWait(200);
		HomeFrom4();
		// goes To Room 2 from room 1 door white line
	}

	printLCD("Exit");	
	
}
//#########################################################################################################



/**********************************************************************************************************
*
* 	Goes Home from Room 1 Trinity
*
***********************************************************************************************************/

//#########################################################################################################
 void HomeFrom1(void)
{
	Forward(-6,UNTIL_TIME,TimerMsCur() + 500);
	
	//SetLeftMotorPWM(-10);
	//SetRightMotorPWM(10);

	sendString("Put Out Flame/r/r"); 

 	//TimerWait(200);
	
	printLCD("Exiting");
		
	ExitRoom();
	
	sendString("Left Room One\r\r"); 
	
	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
 	TimerWait(300);
	
	GoToHeading(((RoomOneExitHeading) + off), TurnToHeadingSpeed); // turns a little more because the motors drift to the right
	GoToHeading(((RoomOneExitHeading) + off), TurnToHeadingSpeed); 
	GoToHeading(((RoomOneExitHeading) + off), TurnToHeadingSpeed); 
	
	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
	TimerWait(300);
	
	Forward(8,UNTIL_TIME,TimerMsCur() + 300);
	
	Forward(8, UNTIL_MIDDLESENSOR_LOWER,SeeWallFront);
	
	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
 	TimerWait(300);
	
	GoToHeading((RoomTwoExitHeading + off), TurnToHeadingSpeed); 
	GoToHeading((RoomTwoExitHeading + off), TurnToHeadingSpeed); 
	GoToHeading((RoomTwoExitHeading + off), TurnToHeadingSpeed);
	
	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
 	TimerWait(300);
	
	Forward(8,UNTIL_TIME,TimerMsCur() + 300);
	
	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
	
	TimerWait(300);

	//RWallFollow(0x30);//			

	RWallFollow(0x30, UNTIL_TIME,TimerMsCur() + 1000);

	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
	TimerWait(300);
	
	//printLCD("L wall follow");

	LWallFollow(0x48, UNTIL_LINESENSOR_LOWER,SeeWallFront);	

	printLCD("2500");

	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
	TimerWait(300);

	GoToHeading((HomeFrom + off), TurnToHeadingSpeed); 
	GoToHeading((HomeFrom + off), TurnToHeadingSpeed); 
	GoToHeading((HomeFrom + off), TurnToHeadingSpeed);

	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
	TimerWait(300);

	Forward(8,UNTIL_TIME,TimerMsCur() + 200);
	sendString("Got To Home Circle\r\r"); 
 
	TimerWait(1000);
}
//#########################################################################################################



/**********************************************************************************************************
*
* 	Goes Home from Room 2 Trinity
*
***********************************************************************************************************/

//#########################################################################################################
 void HomeFrom2(void)
{

	Forward(-6,UNTIL_TIME,TimerMsCur() + 500);

	//SetLeftMotorPWM(-10);
	//SetRightMotorPWM(10);

	sendString("Put Out Flame\r\r"); 
	
 	//TimerWait(200);

	printLCD("Exiting");		

	ExitRoom();

	sendString("Left Room Two\r\r"); 

	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
 	TimerWait(300);

	GoToHeading((RoomTwoExitHeading + off), TurnToHeadingSpeed); 
	GoToHeading((RoomTwoExitHeading + off), TurnToHeadingSpeed); 
	GoToHeading((RoomTwoExitHeading + off), TurnToHeadingSpeed); 

	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
 	TimerWait(200);

	Forward(8,UNTIL_TIME,TimerMsCur() + 300);

	Forward(8, UNTIL_MIDDLESENSOR_LOWER,SeeWallFront);

	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
 	TimerWait(200);

	GoToHeading(((HomeFromTwoHeading) + off), TurnToHeadingSpeed); 
	GoToHeading(((HomeFromTwoHeading) + off), TurnToHeadingSpeed); 
	GoToHeading(((HomeFromTwoHeading) + off), TurnToHeadingSpeed);

	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
 	TimerWait(200);

	Forward(8,UNTIL_TIME,TimerMsCur() + 400);

	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
 	TimerWait(200); 

	GoToHeading(((HomeFromTwoHeading) + off), TurnToHeadingSpeed); 
	GoToHeading(((HomeFromTwoHeading) + off), TurnToHeadingSpeed); 
	GoToHeading(((HomeFromTwoHeading) + off), TurnToHeadingSpeed);

	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
 	TimerWait(200);

	Forward(8,UNTIL_TIME,TimerMsCur() + 400);

	GoToHeading(((HomeFromTwoHeading) + off), TurnToHeadingSpeed); 
	GoToHeading(((HomeFromTwoHeading) + off), TurnToHeadingSpeed); 
	GoToHeading(((HomeFromTwoHeading) + off), TurnToHeadingSpeed);

	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
 	TimerWait(200);

	Forward(8,UNTIL_TIME,TimerMsCur() + 400);

	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
 	TimerWait(200);

	RWallFollow(0x3A,UNTIL_LINESENSOR_LOWER,0x15);// line sensor   when left sees doorway 

	printLCD("2500");

	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
	TimerWait(300);

	GoToHeading((HomeFrom + off), TurnToHeadingSpeed); 
	GoToHeading((HomeFrom + off), TurnToHeadingSpeed); 
	GoToHeading((HomeFrom + off), TurnToHeadingSpeed);

	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
	TimerWait(300);

	Forward(8,UNTIL_TIME,TimerMsCur() + 200); 
	sendString("Got To Home Circle\r\r"); 


	TimerWait(1000);
}
//#########################################################################################################



/**********************************************************************************************************
*
* 	Goes Home from Room 3 Trinity
*
***********************************************************************************************************/

//#########################################################################################################
 void HomeFrom3(void)
{
	Forward(-6,UNTIL_TIME,TimerMsCur() + 500);

	//SetLeftMotorPWM(-10);
	//SetRightMotorPWM(10);

	sendString("Put Out Flame\r\r"); 
	
 	//TimerWait(200);
	
	printLCD("Exiting");		

	ExitRoom();

	sendString("Left Room Three\r\r"); 

	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
 	TimerWait(300);

	GoToHeading((RoomThreeExitHeading + off), TurnToHeadingSpeed); 
	GoToHeading((RoomThreeExitHeading + off), TurnToHeadingSpeed); 
	GoToHeading((RoomThreeExitHeading + off), TurnToHeadingSpeed); 

	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
 	TimerWait(300);

	Forward(6,UNTIL_TIME,TimerMsCur() + 300);

	Forward(6, UNTIL_MIDDLESENSOR_LOWER,SeeWallFront);

	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
 	TimerWait(300);

	GoToHeading(((HomeFromThreeHeading) + off), TurnToHeadingSpeed); 
	GoToHeading(((HomeFromThreeHeading) + off), TurnToHeadingSpeed); 
	GoToHeading(((HomeFromThreeHeading) + off), TurnToHeadingSpeed);

	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
 	TimerWait(200);

	Forward(8,UNTIL_TIME,TimerMsCur() + 600);
	
	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
 	TimerWait(200); 

	GoToHeading(((HomeFromThreeHeading) + off), TurnToHeadingSpeed); 
	GoToHeading(((HomeFromThreeHeading) + off), TurnToHeadingSpeed); 
	GoToHeading(((HomeFromThreeHeading) + off), TurnToHeadingSpeed);

	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
 	TimerWait(200);

	Forward(8,UNTIL_TIME,TimerMsCur() + 600);
	
	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
 	TimerWait(200); 

	GoToHeading(((HomeFromThreeHeading) + off), TurnToHeadingSpeed); 
	GoToHeading(((HomeFromThreeHeading) + off), TurnToHeadingSpeed); 
	GoToHeading(((HomeFromThreeHeading) + off), TurnToHeadingSpeed);

	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
 	TimerWait(200);

	Forward(8,UNTIL_TIME,TimerMsCur() + 600);
	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
 	TimerWait(200); 

	GoToHeading(((HomeFromThreeHeading) + off), TurnToHeadingSpeed); 
	GoToHeading(((HomeFromThreeHeading) + off), TurnToHeadingSpeed); 
	GoToHeading(((HomeFromThreeHeading) + off), TurnToHeadingSpeed);

	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
 	TimerWait(200);

	Forward(8,UNTIL_TIME,TimerMsCur() + 600);
	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
 	TimerWait(200);

	RWallFollow(0x3A,UNTIL_LINESENSOR_LOWER,0X15);// line sensor   
	
	printLCD("2500");

	TimerWait(100);

	GoToHeading((HomeFrom + off), TurnToHeadingSpeed); 
	GoToHeading((HomeFrom + off), TurnToHeadingSpeed); 
	GoToHeading((HomeFrom + off), TurnToHeadingSpeed);

	Forward(8,UNTIL_TIME,TimerMsCur() + 200); 
	sendString("Got To Home Circle\r\r"); 

	TimerWait(1000);}
//#########################################################################################################



/**********************************************************************************************************
*
* 	Goes Home from Room 4 Trinity
*
***********************************************************************************************************/

//#########################################################################################################
 void HomeFrom4(void)
{

	//SetLeftMotorPWM(-10);
	//SetRightMotorPWM(10);

	
 	//TimerWait(200);
		
	if(GotFlame == 1)
	{
		sendString("Put Out Flame\r\r"); 

		Forward(-6,UNTIL_TIME,TimerMsCur() + 500);

		printLCD("Exiting");
		
		ExitRoom();
		
		SetLeftMotorPWM(0);
		SetRightMotorPWM(0);
		TimerWait(300);
	}

	sendString("Left Room Four\r\r"); 

	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
 	TimerWait(300);

	GoToHeading((RoomFourExitHeading + off), TurnToHeadingSpeed); // turns a little more because the motors drift to the right
	GoToHeading((RoomFourExitHeading + off), TurnToHeadingSpeed); 
	GoToHeading((RoomFourExitHeading + off), TurnToHeadingSpeed); 

	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
 	TimerWait(300);

	Forward(8,UNTIL_TIME,TimerMsCur() + 300);

	Forward(8, UNTIL_MIDDLESENSOR_LOWER,SeeWallFront);

	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
 	TimerWait(300);

	GoToHeading((HomeFromFourHeading + off), TurnToHeadingSpeed); 
	GoToHeading((HomeFromFourHeading + off), TurnToHeadingSpeed); 
	GoToHeading((HomeFromFourHeading + off), TurnToHeadingSpeed);

	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
 	TimerWait(300);
	
	Forward(8,UNTIL_TIME,TimerMsCur() + 300);

	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
	TimerWait(300);
	
	//RWallFollow(0x30);//			
	LWallFollow(0x3A,UNTIL_LINESENSOR_LOWER,0x15);// line sensor   when left sees doorway 

	printLCD("2500");

	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
 	TimerWait(300);


	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
 	TimerWait(300);

	Forward(8,UNTIL_TIME,TimerMsCur() + 200); 
	sendString("Got To Home Circle\r\r"); 

	if(GotFlame == 0)
	{
		sendString("Error, Must Have Missed Flame, Starting Again\r\r"); 

		GoToHeading((HomeFrom + off), TurnToHeadingSpeed); 
		GoToHeading((HomeFrom + off), TurnToHeadingSpeed); 
		GoToHeading((HomeFrom + off), TurnToHeadingSpeed);
		ToRoom1();
	}
	
	TimerWait(1000);
}
//#########################################################################################################



/**********************************************************************************************************
*
* 	Leaves the room
*
***********************************************************************************************************/
//#########################################################################################################


void ExitRoom(void)
{

	unsigned char LDist, MDist, RDist;
    //int sensorDifference;
   
    disable_JTAG();                  // Disable JTAG port, making pins available for other use.
    SetupLineSensors();
    InitADC();
   
        // get sensor values		
		RDist = (GetADC(RightDistSensor));
		LDist = (GetADC(LeftDistSensor));
		MDist = (GetADC(MiddleDistSensor));	


	while (GetADC(FrontLineSensor) > 0x15) // Drive until line on the floor
	{	

		RDist = (GetADC(RightDistSensor));
		LDist = (GetADC(LeftDistSensor));
		MDist = (GetADC(MiddleDistSensor));		

	//Turn sharp if too close to wall
		if (MDist > 0x35)
		{
			printLCD("turn");
			SetLeftMotorPWM(0);
			SetRightMotorPWM(5);
			//TimerWait(200);
		}
		/*else if (LDist < 0x15)
		{	
			//printLCD("H  R");		
			SetLeftMotorPWM(6);
			SetRightMotorPWM(1);
		}*/
		/*else if (RDist < 0x15)
		{
			//printLCD("H  L");		
			SetLeftMotorPWM(1);
			SetRightMotorPWM(6);	
		}*/
	
		else if(RDist > (0X35 + 2))
		{
			printLCD("Left");
			SetLeftMotorPWM(6);
			SetRightMotorPWM(7);
		}
	
		else if(RDist < (0X35 - 2))
		{
			printLCD("Right");
			SetLeftMotorPWM(7);
			SetRightMotorPWM(6);
		}		

	//Steer out of the room	
		/*else if (LDist < RDist)	
		{	
			//printLCD("S  R");
			SetLeftMotorPWM(10);
			SetRightMotorPWM(8);		
		}*/
		else 
		{		
			printLCD("Forward");			
			SetLeftMotorPWM(7);
			SetRightMotorPWM(7);
		}
	}
//Reached the threshold
	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);	
}

//#########################################################################################################



/**********************************************************************************************************
###########################################################################################################
###########################################################################################################
###########################################################################################################
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 	

Fire Seaking Functions

^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
###########################################################################################################
###########################################################################################################
###########################################################################################################
***********************************************************************************************************/




/**********************************************************************************************************
*
* 	Goes To The Flame
*
***********************************************************************************************************/
//#########################################################################################################


void GoToFlame(void)						
//will go to the hot pixel
{
	unsigned char	HotPixel,
					Temperature,	
					adcLine;
					
	adcLine = GetADC(FrontLineSensor);
			
	while (!(PINB & 0x20))
	{
		GetHotPixel(&HotPixel,&Temperature);
		adcLine = GetADC(FrontLineSensor);
		if(adcLine > 0x60)	 
		{
			if(Temperature > 2)
			{
				LCD_puts_f(PSTR("Black"),1);
				
				
				//show2hex(HotPixel,Temperature);
				//sendNumber(HotPixel);
				//sendChar(',');
				//sendNumber(Temperature);
				//sendString("\r\n");
				if (HotPixel == 1)
				{
					//LCD_puts_f(PSTR("pixel 1"),1);
					//SetRightMotorPWM(0);
					//SetLeftMotorPWM(0);
					//TimerWait(1000);
					SetRightMotorPWM(2);
					SetLeftMotorPWM(5);
					//TimerWait(100);
				}
				else if (HotPixel == 2 )
				{
					//LCD_puts_f(PSTR("pixel 2"),1);
					//SetRightMotorPWM(0);
					//SetLeftMotorPWM(0);
					//TimerWait(1000);
					SetRightMotorPWM(4);
					SetLeftMotorPWM(6);
					//TimerWait(100);
				}
				else if (HotPixel == 3 )
				{
				//	LCD_puts_f(PSTR("pixel 3"),1);
				//	SetRightMotorPWM(0);
				//	SetLeftMotorPWM(0);		
				//	TimerWait(1000);
					SetRightMotorPWM(5);
					SetLeftMotorPWM(6);
					//TimerWait(100);
				}			
				else if (HotPixel == 4 )
				{
				//	LCD_puts_f(PSTR("pixel 4"),1);
					//SetRightMotorPWM(0);
				//	SetLeftMotorPWM(0);
				//	TimerWait(1000);
					SetRightMotorPWM(6);
					SetLeftMotorPWM(6);
					//TimerWait(100);
				}						
				else if (HotPixel == 5 )
				{
				//	LCD_puts_f(PSTR("pixel 5"),1);
					//SetRightMotorPWM(0);
					//SetLeftMotorPWM(0);
					//TimerWait(1000);
					SetRightMotorPWM(6);
					SetLeftMotorPWM(6);
					//TimerWait(100);
				}
				else if (HotPixel == 6)
				{
					//LCD_puts_f(PSTR("pixel 6"),1);
					//SetRightMotorPWM(0);
					//SetLeftMotorPWM(0);
					//TimerWait(1000);
					SetRightMotorPWM(6);
					SetLeftMotorPWM(5);
					//TimerWait(100);
				}						
				else if (HotPixel == 7)
				{
					//LCD_puts_f(PSTR("pixel 7"),1);
					//SetRightMotorPWM(0);
					//SetLeftMotorPWM(0);
					//TimerWait(1000);
					SetRightMotorPWM(6);
					SetLeftMotorPWM(4);
					//TimerWait(100);
				}		
				else if (HotPixel == 8)
				{
					//LCD_puts_f(PSTR("pixel 8"),1);
					//SetRightMotorPWM(0);
					//SetLeftMotorPWM(0);
					//TimerWait(1000);
					SetRightMotorPWM(5);
					SetLeftMotorPWM(2);
					//TimerWait(100);
				}
				else{
				LCD_puts_f(PSTR("Turning"),1);
				SetRightMotorPWM(5);
				SetLeftMotorPWM(0);
				}
			}
		}
		else
		{
			if(Temperature > 2)
			{
				LCD_puts_f(PSTR("White"),1);
				if (HotPixel == 1)
				{
					//LCD_puts_f(PSTR("pixel 1"),1);
					//SetRightMotorPWM(0);
					//SetLeftMotorPWM(0);
					//TimerWait(1000);
					SetRightMotorPWM(0);
					SetLeftMotorPWM(10);
					//TimerWait(100);
				}
				else if (HotPixel == 2 )
				{
					//LCD_puts_f(PSTR("pixel 2"),1);
					//SetRightMotorPWM(0);
					//SetLeftMotorPWM(0);
					//TimerWait(1000);
					SetRightMotorPWM(0);
					SetLeftMotorPWM(8);
					//TimerWait(100);
				}
				else if (HotPixel == 3 )
				{
				//	LCD_puts_f(PSTR("pixel 3"),1);
				//	SetRightMotorPWM(0);
				//	SetLeftMotorPWM(0);		
				//	TimerWait(1000);
					SetRightMotorPWM(0);
					SetLeftMotorPWM(6);
					//TimerWait(100);
				}			
				else if (HotPixel == 4 )
				{
				//	LCD_puts_f(PSTR("pixel 4"),1);
					//SetRightMotorPWM(0);
				//	SetLeftMotorPWM(0);
				//	TimerWait(1000);
					SetRightMotorPWM(6);
					SetLeftMotorPWM(6);
					//TimerWait(100);
				}						
				else if (HotPixel == 5 )
				{
				//	LCD_puts_f(PSTR("pixel 5"),1);
					//SetRightMotorPWM(0);
					//SetLeftMotorPWM(0);
					//TimerWait(1000);
					SetRightMotorPWM(6);
					SetLeftMotorPWM(6);
					//TimerWait(100);
				}
				else if (HotPixel == 6)
				{
					//LCD_puts_f(PSTR("pixel 6"),1);
					//SetRightMotorPWM(0);
					//SetLeftMotorPWM(0);
					//TimerWait(1000);
					SetRightMotorPWM(6);
					SetLeftMotorPWM(0);
					//TimerWait(100);
				}						
				else if (HotPixel == 7)
				{
					//LCD_puts_f(PSTR("pixel 7"),1);
					//SetRightMotorPWM(0);
					//SetLeftMotorPWM(0);
					//TimerWait(1000);
					SetRightMotorPWM(8);
					SetLeftMotorPWM(0);
					//TimerWait(100);
				}		
				else if (HotPixel == 8)
				{
					//LCD_puts_f(PSTR("pixel 8"),1);
					//SetRightMotorPWM(0);
					//SetLeftMotorPWM(0);
					//TimerWait(1000);
					SetRightMotorPWM(10);
					SetLeftMotorPWM(0);
					//TimerWait(100);
				}
				TimerWait(50);
				SetRightMotorPWM(0);
				SetLeftMotorPWM(0);
				//LCD_puts_f(PSTR("White"),1);
				TimerWait(40);
			}
		}
	}

	SetRightMotorPWM(0);
	SetLeftMotorPWM(0);
	LCD_puts_f(PSTR("Balloon Popped"),1);
	TimerWait(1000);
}
//#########################################################################################################



/**********************************************************************************************************
*
* 	Scans the Room
*
***********************************************************************************************************/
//#########################################################################################################


int ScanRoomForFlame(void)

//this will see if there is flame in the room
//changed to do 540 degrees

{
	unsigned char	//EntryHeading,
					//ExitHeading,
					HotPixel,
					retries,
					Temperature,
					Target;
//					index;
	int 			NumberOfTurns;
	
	
	// get entry heading
	retries	= 0;
	while(!ReadCompass())
	{
		if(++retries > 5)
			return(FALSE);
	}
	
	NumberOfTurns	= 1;
	/*EntryHeading	= Heading;
	
	if (EntryHeading >= 128) {
		ExitHeading = EntryHeading - 128;
	}
	else if(EntryHeading < 128) {
		ExitHeading = EntryHeading + 128;
	}*/
	
	Target	= Heading;
	
	while (!(getkey() == 1))
	{
		// look for hot pixel
		GetHotPixel(&HotPixel,&Temperature);
		if(Temperature > 6) // If there is a flame, returns true
		{
			printLCD("Sees Flame");
			TimerWait(200);				
			//show2hex(HotPixel,Temperature);
		    //TimerWait(2000);		
			//sendNumber(HotPixel);
			//sendChar(',');
			//sendNumber(Temperature);
			//sendString("\r\n");
			Target = Target + ((4 - HotPixel) * 3); // try to leave function centered on flame
			GoToHeading(Target,3);
//			();
			return(TRUE);
		}
		else
		{
			Target = Target + 14; // 28 is 40 degrees, range of the sensor
			GoToHeading(Target,3);
			NumberOfTurns++;
			show1hex(NumberOfTurns);
			if(NumberOfTurns > 11) // If a full turn is completed, return to orignal heading
			{	
				//show2hex(EntryHeading, ExitHeading);
				//TimerWait(5000);
				printLCD("No Flame");
				TimerWait(200);												
				return(FALSE);
			}
		}
	}
	return(FALSE);
}
//#########################################################################################################



/**********************************************************************************************************
###########################################################################################################
###########################################################################################################
###########################################################################################################
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 	

Movement Functions

^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
###########################################################################################################
###########################################################################################################
###########################################################################################################
***********************************************************************************************************/

/**********************************************************************************************************
*
* 	Follows Right Wall
*
***********************************************************************************************************/
//#########################################################################################################



void RWallFollow(int TargetDistance,int TerminatingCondition,unsigned long TerminatingValue)
{
	unsigned char adcL, adcM, adcR;
   
	do
	{
		adcR = GetADC(RightDistSensor);
		adcM = GetADC(MiddleDistSensor);
		adcL = GetADC(LeftDistSensor);
		//show2(adcM, adcR);
		
/*		if(adcR > TargetDistance)
		{	
			Diff = (TargetDistance - adcR)/16;
			if (Diff > 9)
			{
				Diff = 10;
			}
			SetLeftMotorPWM(10 - Diff);
			SetRightMotorPWM(10);
			
		}	
		else if(adcR < TargetDistance)
		{	
			Diff = adcR -TargetDistance;
			if (Diff > 9)
			{
				Diff = 10;
			}
			SetLeftMotorPWM(10);
			SetRightMotorPWM(10 -Diff);
			
		}
		else
		{
			SetLeftMotorPWM(10);
			SetRightMotorPWM(10);
		}
		*/
		if(adcR > (TargetDistance + 25))
		{
			SetLeftMotorPWM(5);
			SetRightMotorPWM(7);
		//	sendString("Hard Left\r\r"); 
		
		}
		else if(adcR > (TargetDistance + 5))
		{
			SetLeftMotorPWM(9);
			SetRightMotorPWM(10);
		//	sendString("Soft Left\r\r"); 

		}
		else if(adcR > (TargetDistance))
		{
			SetLeftMotorPWM(10);
			SetRightMotorPWM(10);
		//	sendString("Softer Left\r\r"); 

		}
		/*else if(adcR > (TargetDistance + 2))
		{
			SetLeftMotorPWM(10);
			SetRightMotorPWM(10);
		}		
		else if(adcR < (TargetDistance - 6))
		{
			SetLeftMotorPWM(10);
			SetRightMotorPWM(8);	
		}*/
		else if(adcR < (TargetDistance - 25))
		{
			SetLeftMotorPWM(7);
			SetRightMotorPWM(5);
		//	sendString("Hard Right\r\r"); 

		}
		else if(adcR < (TargetDistance - 5))
		{
			SetLeftMotorPWM(10);
			SetRightMotorPWM(9);
		//	sendString("Soft Right\r\r"); 

		}		
		else if(adcR < (TargetDistance))
		{
			SetLeftMotorPWM(10);
			SetRightMotorPWM(10);
		//	sendString("Softer Right\r\r"); 

		}
		else
		{
			SetLeftMotorPWM(10);
			SetRightMotorPWM(10);
		}
		


	} while(!CheckTerminatingConditions(TerminatingCondition,TerminatingValue));
	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
}




/**********************************************************************************************************
*
* 	Follows Left Wall
*
***********************************************************************************************************/

//#########################################################################################################


void LWallFollow(int TargetDistance,int TerminatingCondition,unsigned long TerminatingValue)
{
	unsigned char adcL, adcM, adcR;
   
	do
	{
		adcR = GetADC(RightDistSensor);
		adcM = GetADC(MiddleDistSensor);
		adcL = GetADC(LeftDistSensor);
		//show2(adcL, adcM);
		
		if(adcL < (TargetDistance - 25)) // was 6
		{
			SetLeftMotorPWM(5);
			SetRightMotorPWM(7);
		//	sendString("Hard Left\r\r"); 
		
		}		
		else if(adcL < (TargetDistance - 5)) // was 6
		{
			SetLeftMotorPWM(9);
			SetRightMotorPWM(10);
		//	sendString("Soft Left\r\r"); 
		
		}		
		else if(adcL < (TargetDistance)) // was 6
		{
			SetLeftMotorPWM(10);
			SetRightMotorPWM(10);
		//	sendString("Softer Left\r\r"); 

		}
		/********************************************************/
		else if(adcL > (TargetDistance + 25)) // was 6
		{
			SetLeftMotorPWM(7);
			SetRightMotorPWM(5);
		//	sendString("Hard Right\r\r"); 

		}
		else if(adcL > (TargetDistance + 5)) // was 6
		{
			SetLeftMotorPWM(10);
			SetRightMotorPWM(9);
		//	sendString("Soft Right\r\r"); 
		
		}
		else if(adcL > (TargetDistance)) // 2
		{
			SetLeftMotorPWM(10);
			SetRightMotorPWM(10);
		//	sendString("Softer Right\r\r"); 

		}		
		else
		{
			SetLeftMotorPWM(10);
			SetRightMotorPWM(10);
		}
		/*else if(adcL < (TargetDistance - 11)) // was 6
		{
			SetLeftMotorPWM(3);
			SetRightMotorPWM(10);
		}		
		else if(adcL < (TargetDistance - 7)) // was 6
		{
			SetLeftMotorPWM(5);
			SetRightMotorPWM(10);
		}		
		else if(adcL < (TargetDistance - 6)) // was 4
		{
			SetLeftMotorPWM(6);
			SetRightMotorPWM(10);
		}
		else */
		
	//	else if(adcL < (TargetDistance - 4)) // was 6
	//	{
	//		SetLeftMotorPWM(8);
	//		SetRightMotorPWM(10);
	//	}		
	//	else if(adcL < (TargetDistance - 3)) // was 6
	//	{
	//		SetLeftMotorPWM(9);
	//		SetRightMotorPWM(10);
	//	}		
	//	else if(adcL < (TargetDistance - 2)) // was 2
	//	{
	//		SetLeftMotorPWM(10);
	//		SetRightMotorPWM(10);
	//	}
		
		/*else if(adcL > (TargetDistance + 10)) // was 6
		{
			SetLeftMotorPWM(10);
			SetRightMotorPWM(1);
		}
		else if(adcL > (TargetDistance + 9)) // was 6
		{
			SetLeftMotorPWM(10);
			SetRightMotorPWM(2);
		}
		else if(adcL > (TargetDistance + 8)) // was 6
		{
			SetLeftMotorPWM(10);
			SetRightMotorPWM(3);
		}
		else if(adcL > (TargetDistance + 7)) // was 6
		{
			SetLeftMotorPWM(10);
			SetRightMotorPWM(4);
		}
		else if(adcL > (TargetDistance + 6)) // was 4
		{
			SetLeftMotorPWM(10);
			SetRightMotorPWM(5);
		}
		else */
	//	if(adcL > (TargetDistance + 5)) // was 6
	//	{
	//		SetLeftMotorPWM(10);
	//		SetRightMotorPWM(6);
	//	}

	} while(!CheckTerminatingConditions(TerminatingCondition,TerminatingValue));
	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
}
//#########################################################################################################


/**********************************************************************************************************
*
* 	Go To Heading
*
***********************************************************************************************************/
//#########################################################################################################


int GoToHeading(unsigned char TargetHeading,unsigned char Speed)
{
	unsigned char retries=0, diff=0;
	
	if(!ReadCompass())
	{
		SetRightMotorPWM(0);
		SetLeftMotorPWM(0);
		printLCD("bad compass reading");
		TimerWait(10000);
		return(FALSE);
	}
	if (Heading > TargetHeading)
		{
		diff = Heading - TargetHeading;
		}
	else if (Heading == TargetHeading)
		{
		diff = 0;
		}
	else{
		diff = TargetHeading - Heading;
		}
	while (diff >= 4)
	{//Left Turns
		//Left turns that don't cross zero
		if((Heading > TargetHeading) && ((Heading - TargetHeading) <= 128)){	
			while((Heading > TargetHeading) && ((Heading -TargetHeading)>4)&& ((Heading - TargetHeading) <= 128))
			{//Turn at decreasing speed until within 2 of Target
				//printLCD("Left1");
				//diff = Heading - TargetHeading;
				//show1hex(diff);
				//SetLeftMotorPWM(-((diff/(15-Speed))+2));
				//SetRightMotorPWM((diff/(15-Speed))+2);
				retries	= 0;
				SetLeftMotorPWM(-Speed);
				SetRightMotorPWM(Speed);
				while(!ReadCompass())
				{
					SetRightMotorPWM(0);
					SetLeftMotorPWM(0);
					printLCD("Compass Recheck");
					TimerWait(3000);					
					if(++retries > 5)
					{
						printLCD("L1Error");
						TimerWait(3000);					
						return(FALSE);
					}
				}
			}
			SetRightMotorPWM(0);
			SetLeftMotorPWM(0);
			printLCD("L1 Done");
			return(TRUE);//Exit
		}	
		//Left turns that cross zero 
		else if	((Heading < TargetHeading) && ((TargetHeading - Heading) >= 128)){
			while((Heading < TargetHeading) && ((255 - TargetHeading + Heading)>4)&& ((TargetHeading - Heading) >= 128))
			{//Turn at decreasing speed until within 2 of traget
				//printLCD("Left2");	
				//diff = (255 - TargetHeading + Heading);
				//show1hex(diff);
				//SetLeftMotorPWM(-((diff/(15-Speed))+2));
				//SetRightMotorPWM((diff/(15-Speed))+2);
				SetLeftMotorPWM(-Speed);
				SetRightMotorPWM(Speed);
				retries	= 0;
				while(!ReadCompass())
				{
					SetRightMotorPWM(0);
					SetLeftMotorPWM(0);
					printLCD("Compass Recheck");
					TimerWait(3000);					
					if(++retries > 5)
					{
						printLCD("L2Error");
						TimerWait(3000);					
						return(FALSE);
					}
				}
			}
			//SetRightMotorPWM(0);
			//SetLeftMotorPWM(0);
			printLCD("L2 Done");		
		}
	//Right Turns
		//Right turn that do not cross zero
		else if((Heading < TargetHeading) && ((TargetHeading - Heading) < 128)){
			while((Heading < TargetHeading) && ((TargetHeading - Heading)>4) && ((TargetHeading - Heading) < 128))
			{//Turn at descreasing speed until within 2 of Target
				//printLCD("Right1");
				//diff = (TargetHeading - Heading);
				//show1hex(diff);
				//SetLeftMotorPWM((diff/(15-Speed))+2);
				//SetRightMotorPWM(-((diff/(15-Speed))+2));		
				SetLeftMotorPWM(Speed);
				SetRightMotorPWM(-Speed);
				retries	= 0;
				while(!ReadCompass())
				{
					SetRightMotorPWM(0);
					SetLeftMotorPWM(0);
					printLCD("Compass Recheck");
					TimerWait(3000);					
					if(++retries > 5)
					{
						printLCD("R1Error");
						TimerWait(3000);					
						return(FALSE);
					}
				}
			}		
			SetRightMotorPWM(0);
			SetLeftMotorPWM(0);	
			printLCD("R1 Done");
			return(TRUE);//exit
		}	
		//right turn that do cross zero
		else if	((Heading > TargetHeading) && ((Heading - TargetHeading) > 128)){
			while((Heading > TargetHeading) && ((255 - Heading + TargetHeading)>4) && ((Heading - TargetHeading) > 128))
			{//Turn with decreasing speed until within 2 of target
				//printLCD("Right2");
				//diff = (255 - Heading + TargetHeading);
				//show1hex(diff);
				//SetLeftMotorPWM((diff/(15-Speed))+2);
				//SetRightMotorPWM(-((diff/(15-Speed))+2));
				SetLeftMotorPWM(Speed);
				SetRightMotorPWM(-Speed);
				retries	= 0;
				while(!ReadCompass())
				{						
					SetRightMotorPWM(0);
					SetLeftMotorPWM(0);
					printLCD("Compass Recheck");
					TimerWait(3000);
					if(++retries > 5)
					{
						printLCD("R2Error");
						TimerWait(3000);					
						return(FALSE);
					}
				}
			}
			//SetRightMotorPWM(0);
			//SetLeftMotorPWM(0);
			printLCD("R2 Done");		
		}
		else{
			//else bad things
			SetRightMotorPWM(0);//to bottom outside while loop
			SetLeftMotorPWM(0);
			printLCD("Should never happen");
			TimerWait(5000);
			show2hex(Heading,TargetHeading);
			TimerWait(5000);
			return(FALSE);
			}
		SetRightMotorPWM(0);//to bottom outside while loop
		SetLeftMotorPWM(0);
		//printLCD("Target");
		//TimerWait(500);
		return(TRUE);
	}
	SetRightMotorPWM(0);//to bottom outside while loop
	SetLeftMotorPWM(0);
	//printLCD("Target");
	//TimerWait(500);
	return(TRUE);
}
//#########################################################################################################


/**********************************************************************************************************
*
* 	Turn - Turns for a time
*
***********************************************************************************************************/
//#########################################################################################################


/*void RTurn(int Speed,int TerminatingCondition,unsigned long TerminatingValue)
{
	do
	{
		SetLeftMotorPWM(Speed);
		SetRightMotorPWM(Speed-3);
	} while(!CheckTerminatingConditions(TerminatingCondition,TerminatingValue));

	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
}*/
//#########################################################################################################


/**********************************************************************************************************
*
* 	Turn - Turns for a time
*
***********************************************************************************************************/
//#########################################################################################################


/*void LTurn(int Speed,int TerminatingCondition,unsigned long TerminatingValue)
{
	do
	{
		SetLeftMotorPWM(Speed-3);
		SetRightMotorPWM(Speed);
	} while(!CheckTerminatingConditions(TerminatingCondition,TerminatingValue));

	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
}*/
//#########################################################################################################


/**********************************************************************************************************
*
* 	Turns 90 Degrees
*
***********************************************************************************************************/
//#########################################################################################################


/*void RTurn90(void)
{
	unsigned char	TargetHeading;

	if (ReadCompass())
	{
		TargetHeading = Heading + 28;
	
		if(!GoToHeading(TargetHeading,7))
		{
			printLCD("Error");
		}
		if(!GoToHeading(TargetHeading,7))
		{
			printLCD("Error");
		}
		if(!GoToHeading(TargetHeading,7))
		{
			printLCD("Error");
		}		
		SetRightMotorPWM(0);
		SetLeftMotorPWM(0);	
	}	
}*/
//#########################################################################################################



/**********************************************************************************************************
*
* 	Forward - Goes forward for a time
*
***********************************************************************************************************/
//#########################################################################################################


void Forward(int Speed,int TerminatingCondition,unsigned long TerminatingValue)
{
	do
	{
		SetLeftMotorPWM(Speed);
		SetRightMotorPWM(Speed);
	} while(!CheckTerminatingConditions(TerminatingCondition,TerminatingValue));

	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
}
//#########################################################################################################


/**********************************************************************************************************
###########################################################################################################
###########################################################################################################
###########################################################################################################
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 	

Sensor Displays

^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
###########################################################################################################
###########################################################################################################
###########################################################################################################
***********************************************************************************************************/



/**********************************************************************************************************
*
* 	Shows the Raw value for the Distance sensors
*
***********************************************************************************************************/
//#########################################################################################################


/*void showDist(void)  
{
    unsigned char adcLDist, adcMDist, adcRDist;
	   
    disable_JTAG();                  // Disable JTAG port, making pins available for other use.
    SetupLineSensors();
    InitADC();
   
    while (!(getkey() == 1))       //do the folling block until enter is pressed
    {
        // get line sensor values
		adcRDist = GetADC(RightDistSensor);
		adcLDist = GetADC(LeftDistSensor);
		adcMDist = GetADC(MiddleDistSensor);
		
        show3hex(adcLDist, adcMDist, adcRDist);
        TimerWait(100);
    }
}*/
//#########################################################################################################


/**********************************************************************************************************
*
* 	Shows the Linearized value for the Distance sensors
*
***********************************************************************************************************/
//#########################################################################################################


/*void showLinearDist(void)  
{
    unsigned char adcLDist, adcMDist, adcRDist;
	unsigned char LDist, MDist, RDist;
    //int sensorDifference;
   
    disable_JTAG();                  // Disable JTAG port, making pins available for other use.
    SetupLineSensors();
    InitADC();
   
    while (!(getkey() == 1))       //do the folling block until enter is pressed
    {
        // get line sensor values
		adcRDist = GetADC(RightDistSensor);
		adcLDist = GetADC(LeftDistSensor);
		adcMDist = GetADC(MiddleDistSensor);
		
		RDist = (2000/adcRDist);
		LDist = (2000/adcLDist);
		MDist = (2000/adcMDist);	
		//RDist = ((7947/(adcRDist-15))-4);
		//LDist = ((7947/(adcLDist-15))-4);	
//		sensorDifference = (LDist - RDist);
//      show2(adcL, sensorDifference);
        show3hex(LDist,MDist,RDist);
        TimerWait(100);
    }
}*/
//#########################################################################################################



/**********************************************************************************************************
###########################################################################################################
###########################################################################################################
###########################################################################################################
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 	

Fire Fighting Tests

^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
###########################################################################################################
###########################################################################################################
###########################################################################################################
***********************************************************************************************************/




/**********************************************************************************************************
*
* 	Threshold Test - Tests the Line Sensor by driving and sending out the sensor value.
*
***********************************************************************************************************/

//##########################################################################################################
/*void Thresholdtest(void)
{
	
	unsigned char adcLine;
	
    while (!(getkey() == 1))
    { 
	SetLeftMotorPWM(10);
    SetRightMotorPWM(10);
	adcLine = GetADC(FrontLineSensor);
	show1hex(adcLine);	
	sendNumber(adcLine);
	sendString("\r\n");
	TimerWait(100);
	}

}*/
//##########################################################################################################





/**********************************************************************************************************
*
*	 Motor Test - Tests the Motors By Driving them in diffrent directions.
*
***********************************************************************************************************/

//##########################################################################################################
/*void PWM_Test(void)
{
    while (!(getkey() == 1))
    { 	 
	
	    SetLeftMotorPWM(0);
        SetRightMotorPWM(10);
        TimerWait(500);
		
	    SetLeftMotorPWM(10);
        SetRightMotorPWM(0);
        TimerWait(500);

	    SetLeftMotorPWM(10);
        SetRightMotorPWM(10);
        TimerWait(500);
		
	
	}
    // Done
		SetLeftMotorPWM(0);
        SetRightMotorPWM(0);
}*/
//##########################################################################################################




/**********************************************************************************************************
*
*	 Heading test - Turns to diffrent points using the compass
*
***********************************************************************************************************/

//##########################################################################################################
/*void HeadingTest(void) 
{
	unsigned char TargetHeading = 255 + 60;
	while (1)
	{
	GoToHeading(TargetHeading,5);
//	show1hex(Heading);
//	TimerWait(2000);
	
//	TargetHeading = 0xC0;
//	GoToHeading(TargetHeading,10);
//	show1hex(Heading);
//	TimerWait(2000);

//	TargetHeading = 0x20;
//	GoToHeading(TargetHeading,10);
//	show1hex(Heading);
//	TimerWait(2000);

//	TargetHeading = 0x60;
//	GoToHeading(TargetHeading,10);
//	show1hex(Heading);
//	TimerWait(2000);

//	TargetHeading = 0x20;
//	GoToHeading(TargetHeading,10);
//	show1hex(Heading);
//	TimerWait(2000);

//	TargetHeading = 0xA8;
//	GoToHeading(TargetHeading,10);
//	show1hex(Heading);
//	TimerWait(2000);

//	TargetHeading = 0x28;
//	GoToHeading(TargetHeading,10);
//	show1hex(Heading);
//	TimerWait(2000);
	}

}*/
//##########################################################################################################





/**********************************************************************************************************
*
*	 Turn Test - Keeps Turning 64 from were it started.
*
***********************************************************************************************************/

//##########################################################################################################
/*void TurnTest(void)
{
	unsigned char	TargetHeading;


	TargetHeading	= ReadCompass() ? Heading + 64 : 0;

	while (!(getkey() == 1))
	{
		if(ReadCompass())
		{
			show1hex(Heading);

			if(Heading < TargetHeading)
			{
				SetRightMotorPWM(8);
				SetLeftMotorPWM(-8);
				show1hex(TargetHeading);
				//TimerWait(20);
			}
			if(Heading > TargetHeading)
			{
				SetRightMotorPWM(-8);
				SetLeftMotorPWM(8);
				show1hex(TargetHeading);
				//TimerWait(20);
			}
			if(-10 < (Heading-TargetHeading) && 10 > (Heading-TargetHeading))
			{
				printLCD("Home");
				TargetHeading    += 64;
				SetRightMotorPWM(0);
				SetLeftMotorPWM(0);
				TimerWait(3000);
			}
		}
		else
		{
			printLCD("Error");
		}
	}
}*/
//##########################################################################################################




/**********************************************************************************************************
*
*	 Line Sensor Test - Keeps Turning 64 from were it started.
*
***********************************************************************************************************/

//##########################################################################################################
/*void LineDisplay(void)
{
	printLCD("Sending To Computer");
	unsigned char adcLine;
	while (!(getkey() == 1))
	{
		adcLine = GetADC(FrontLineSensor);
		show1hex(adcLine);
		sendNumber(adcLine);
		sendString("\r\n");
		//TimerWait(200);
	}
}*/
//##########################################################################################################



/**********************************************************************************************************
*
*	 Line Test - Drives until line.
*
***********************************************************************************************************/

//##########################################################################################################
/*void LineTest(void)
{
	unsigned char adcLine;
	while (!(getkey() == 1))
	{
		adcLine = GetADC(FrontLineSensor);
		show1hex(adcLine);
	//	SetLeftMotorPWM(10);
	//	SetRightMotorPWM(10);
		
	//	if(adcLine < 0xEE)
	//	{
	//	SetLeftMotorPWM(0);
	//	SetRightMotorPWM(0);	
	//	TimerWait(2000);	
	//	}		
	//}
}*/

	
/**********************************************************************************************************
###########################################################################################################
###########################################################################################################
###########################################################################################################
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 	

Other Functions

^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
###########################################################################################################
###########################################################################################################
###########################################################################################################
***********************************************************************************************************/





/**********************************************************************************************************
*
* 	A FireFighting Demo For Showing Off
*
***********************************************************************************************************/
//#########################################################################################################


/*void FlameDemo(void)
{
	while (!(getkey() == 1))
	{
	unsigned char	HotPixel,
					Temperature;
		GetHotPixel(&HotPixel,&Temperature);

		if(Temperature > 6) // If there is a flame, returns true
		{
			show2hex(HotPixel,Temperature);
			sendNumber(HotPixel);
			sendChar(',');
			sendNumber(Temperature);
			sendString("\r\n");
			if (HotPixel == 1)
			{
				//printLCD("pixel 1");
				//SetRightMotorPWM(0);
				//SetLeftMotorPWM(0);
				//TimerWait(1000);
				SetRightMotorPWM(7);
				SetLeftMotorPWM(10);
				TimerWait(10);
				sendString("Pixel 1");	
				sendChar(',');
				sendNumber(Temperature);	
				sendString("\r\n");
			}
			else if (HotPixel == 2 )
			{
				//printLCD("pixel 2");
				//SetRightMotorPWM(0);
				//SetLeftMotorPWM(0);
				//TimerWait(1000);
				SetRightMotorPWM(8);
				SetLeftMotorPWM(10);
				TimerWait(10);
				sendString("Pixel 2");	
				sendChar(',');
				sendNumber(Temperature);	
				sendString("\r\n");
			}
			else if (HotPixel == 3 )
			{
			//	printLCD("pixel 3");
			//	SetRightMotorPWM(0);
			//	SetLeftMotorPWM(0);		
			//	TimerWait(1000);
				SetRightMotorPWM(9);
				SetLeftMotorPWM(10);
				TimerWait(10);
				sendString("Pixel 3");	
				sendChar(',');
				sendNumber(Temperature);	
				sendString("\r\n");			
			}			
			else if (HotPixel == 4 )
			{
			//	printLCD("pixel 4");
				//SetRightMotorPWM(0);
			//	SetLeftMotorPWM(0);
			//	TimerWait(1000);
				SetRightMotorPWM(10);
				SetLeftMotorPWM(10);
				TimerWait(10);
				sendString("Pixel 4");	
				sendChar(',');
				sendNumber(Temperature);	
				sendString("\r\n");
			}						
			else if (HotPixel == 5 )
			{
			//	printLCD("pixel 5");
				//SetRightMotorPWM(0);
				//SetLeftMotorPWM(0);
				//TimerWait(1000);
				SetRightMotorPWM(10);
				SetLeftMotorPWM(10);
				TimerWait(10);
				sendString("Pixel 5");	
				sendChar(',');
				sendNumber(Temperature);	
				sendString("\r\n");
			}
			else if (HotPixel == 6)
			{
				//printLCD("pixel 6");
				//SetRightMotorPWM(0);
				//SetLeftMotorPWM(0);
				//TimerWait(1000);
				SetRightMotorPWM(10);
				SetLeftMotorPWM(9);
				TimerWait(10);
				sendString("Pixel 6");	
				sendChar(',');
				sendNumber(Temperature);	
				sendString("\r\n");
			}						
			else if (HotPixel == 7)
			{
				//printLCD("pixel 7");
				//SetRightMotorPWM(0);
				//SetLeftMotorPWM(0);
				//TimerWait(1000);
				SetRightMotorPWM(10);
				SetLeftMotorPWM(8);
				TimerWait(10);
				sendString("Pixel 7");	
				sendChar(',');
				sendNumber(Temperature);	
				sendString("\r\n");
			}		
			else if (HotPixel == 8)
			{
				//printLCD("pixel 8");
				//SetRightMotorPWM(0);
				//SetLeftMotorPWM(0);
				//TimerWait(1000);
				SetRightMotorPWM(10);
				SetLeftMotorPWM(7);
				TimerWait(10);
				sendString("Pixel 8");	
				sendChar(',');
				sendNumber(Temperature);	
				sendString("\r\n");
			}

		//	if(PINB & 0x20)				// check for balloon pop
		//	{
		//		SetRightMotorPWM(0);
		//		SetLeftMotorPWM(0);
		//		TimerWait(1000);
		//		break;
		//

			else
			{
				break;
			}
		}
		else
		{
			SetRightMotorPWM(7);
			SetLeftMotorPWM(-7);
			printLCD("no");
			sendString("No");		
			sendString("\r\n");
		}
	}
}*/

//#########################################################################################################



/**********************************************************************************************************
*
* 	Terminating Conditions - a while loop with diffrent terminating conditions.
*
***********************************************************************************************************/
//#########################################################################################################

int CheckTerminatingConditions(int TerminatingCondition,unsigned long TerminatingValue)
{
	unsigned char LDist, MDist, RDist;

	RDist = (2000/GetADC(RightDistSensor));
	MDist = (2000/GetADC(MiddleDistSensor));
	LDist = (2000/GetADC(LeftDistSensor));
	
	switch(TerminatingCondition)
	{
	case UNTIL_KEYPRESS:
		if(getkey() == TerminatingValue)
			return(TRUE);
		
		break;

	case UNTIL_RIGHTSENSOR_GREATER:
		if(GetADC(RightDistSensor) > TerminatingValue)
			return(TRUE);
		
		break;

	case UNTIL_MIDDLESENSOR_GREATER:
		if(GetADC(MiddleDistSensor) > TerminatingValue)
			return(TRUE);
		
		break;

	case UNTIL_LEFTSENSOR_GREATER:
		if(GetADC(LeftDistSensor) > TerminatingValue)
			return(TRUE);
		
		break;
	
	case UNTIL_LINESENSOR_GREATER:
		if(GetADC(FrontLineSensor) > TerminatingValue)
			return(TRUE);
		
		break;

	case UNTIL_RIGHTSENSOR_LOWER:
		if(GetADC(RightDistSensor) < TerminatingValue)
			return(TRUE);
		
		break;

	case UNTIL_MIDDLESENSOR_LOWER:
		if((2000/GetADC(MiddleDistSensor)) < TerminatingValue)
			return(TRUE);
		
		break;

	case UNTIL_LEFTSENSOR_LOWER:
		if(GetADC(LeftDistSensor) < TerminatingValue)
			return(TRUE);
		
		break;
	
	case UNTIL_LINESENSOR_LOWER:
		if(GetADC(FrontLineSensor) < TerminatingValue)
			return(TRUE);
		
		break;

	case UNTIL_TIME:
		if(TimerMsCur() > TerminatingValue)
			return(TRUE);
		
		break;
	}

	return(FALSE);

}
//#########################################################################################################

/*void DisplayFlame(void)						
//will go to the hot pixel
{
	unsigned char	HotPixel,
					Temperature;						
	while (!(getkey() == 1))	
	{
		GetHotPixel(&HotPixel,&Temperature);
		if(Temperature > 2)
		{		
			//sendString("\r\n");
			if (HotPixel == 1)
			{
				sendString("0=======\r");
			}
			else if (HotPixel == 2 )
			{
				sendString("=0======\r");

			}
			else if (HotPixel == 3 )
			{
				sendString("==0=====\r");

			}			
			else if (HotPixel == 4 )
			{
				sendString("===0====\r");

			}						
			else if (HotPixel == 5 )
			{
				sendString("====0===\r");

			}
			else if (HotPixel == 6)
			{
				sendString("=====0==\r");

			}						
			else if (HotPixel == 7)
			{
				sendString("======0=\r");

			}		
			else if (HotPixel == 8)
			{
				sendString("=======0\r");

			}
			else
			{
				sendString("No Flame\r\r");
			}
		}
	}
}*/
